#pragma once
namespace cvLib
{
	public class HMPara
	{	
	public:
		// Create Template
		const bool				hasROITemplate;
		const bool				hasInteractantMaskT;
		const bool				hasBestViewMaskT;
		const eT  				deltaOri;
		const eT  				interactionCenterDis;
		const int 				sliceNum;
		const int 				bViewMaskMaxR;
		const int 				bViewMaskN;

		// Matrix Size
		const int				szGround;
		const int				szRadiusROI;
		const int				szROIMatrix;

		// Parameter
		const eT				sigmaXSquare;
		const eT				sigmaYSquare;	
		const eT				decayK;
		const eT				decayCoef;
		const eT				normValue;

		const eT				interactantThres;
		const eT				interactionCenterThres;
		const int				postProcR;
		const int				szFilter;
		const int				interactantRadius;
		const int				deltaR;

		HMPara():
			hasROITemplate				(true),
			hasInteractantMaskT			(true),
			hasBestViewMaskT			(true),
			deltaOri					(45.0f),
			interactionCenterDis		(45),
			sliceNum					(360),
			bViewMaskMaxR				(500),
			bViewMaskN					(6),

			szGround					(1000),
			szRadiusROI					(350),
			szROIMatrix					(2*350+1),

			sigmaXSquare				(6230.0f),
			sigmaYSquare				(6230.0f),	
			decayK						(0.1f),
			decayCoef					(exp(-0.1f)),
			normValue					(1 - exp(-0.1f)),

			interactantThres			(0.15f),
			interactionCenterThres		(0.5f),
			postProcR					(150),
			szFilter					(30),
			interactantRadius			(100),
			deltaR						(200)
		{

		}

	};
	private class HM
	{
	private:
#pragma region Variables

		arma::Mat<eT> RoILoad;
		arma::Mat<eT> groundTemplate;
		arma::Mat<eT> zeroGround;
		arma::Mat<eT> iFoIThres;
		arma::Cube<eT> personRoICube;

		arma::Mat<eT> RoomS;
		arma::Mat<eT> RoomST;	
		arma::Cube<eT> personST;
		arma::Mat<eT> RoomBV;

		arma::Mat<unsigned int> iFoINum;
		arma::Mat<unsigned int> iFoIMaskMulti;
		arma::Mat<eT> iFoIMaskDiv;   

		arma::Col<int> interactionCenterX; 
		arma::Col<int> interactionCenterY;  
		int interactionCenterCnt;
		arma::Mat<eT> interactionValue;

		//eT interactionValue[MaxNumPeople/2][MaxNumPeople];
		arma::Mat<eT> interactantCircleMask;

		arma::Mat<eT> bViewMask;
		arma::Col<eT> bValue; // Output Array
		arma::Cube<eT> sectorMaskTemplate;

		// gantian Load Data <1>
		/*arma::Cube<eT> dataLoad;
		arma::Col<int> dataLocX;       
		arma::Col<int> dataLocY;
		arma::Col<int> dataLocOri;*/

		// result
		/*arma::Col<int> resCenterNo;
		arma::Cube<int> resInteractant;
		arma::Cube<int> resCenterLoc;*/

		std::ostringstream convertS;
		arma::wall_clock timer_arma;

#pragma endregion
		void loadPeopleInfo
			(
			Cube<eT>&				dataLoad, 
			int&					szSeq, 
			int&					szPeople
			);

		void updatePeopleInfo
			(
			const int&				frameNo, 
			const Cube<eT>&			dataLoad,	//input
			const int&				szXLim, 
			const int&				szYLim, // check validity
			Col<int>&				locX,
			Col<int>&				locY, 
			Col<int>&				locOri,
			int&					cntPeople
			);	//output

		inline bool areClockwise(const eT& v1X, const eT& v1Y, const eT& v2X, const eT& v2Y){return ((-v1X*v2Y + v1Y*v2X)>=-EPS);}
		bool convolve2D(const arma::Mat<eT>& in, arma::Mat<eT>& out, const int& sizeRow, const int& sizeCol, const arma::Mat<eT>& kernel, const int& kernelSizeX, const int& kernelSizeY);

		void validatePeopleInfo
			(
			Col<eT>&				locX,
			Col<eT>&				locY, 
			Col<eT>&				locOri,
			const int&				cntPeople,		
			const HMPara&			p
		);

		void personROIMask
			(
			arma::Mat<eT>&			personROIMatrix,
			const eT&				ori,
			const eT&				radius,
			const HMPara&			p
			);

		bool isPointInCircularSector
			(
			const eT&				pX, 
			const eT&				pY, 
			const eT&				cX, 
			const eT&				cY,
			const eT&				sectorSX, 
			const eT&				sectorSY, 
			const eT&				sectorEX, 
			const eT&				sectorEY, 
			const eT&				squareRadius
			);

		arma::Mat<eT> getGaussianMatrix
			(
			const HMPara& p	
			);

		void getBestViewValue
			(
			arma::Col<eT>&			retBValue,
			const arma::Mat<eT>&	heatMap, 
			const int&				locX, 
			const int&				locY, 
			const Cube<eT>&			sectorMaskTemplate, 
			const int&				innerR,
			const HMPara&			p
			);

		bool convolveSep
			(		
			arma::Mat<eT>&			resOut, 		
			const arma::Mat<eT>&	in, 
			const arma::Col<eT>&	kernel,
			const HMPara			p				
			);

		void smoothImage
			(
			arma::Mat<eT>&			A, 			
			const HMPara&			p
			);

		void interactionCenterSeeking
			(						
			arma::Col<int>&			retX, 
			arma::Col<int>&			retY, 
			int&					retCnt,
			const arma::Mat<eT>&	hmICS, 		
			const HMPara&			p
			);

		eT getInteractionValue
			(
			const arma::Mat<eT>&	heatMap, 
			const int&				locRow, 
			const int&				locCol, 
			const arma::Mat<eT>&	maskMatrix, 			
			const HMPara&			p
			);

	public :

		HMPara						para;
		int							szCurPeopleHM;		
		void heatMapCalc
			(
			Col<eT>&				locX, 
			Col<eT>&				locY, 
			Col<eT>&				locOri,
			const HMPara&			p
			);

		HM()			
		{

			RoILoad.set_size(para.szROIMatrix,para.szROIMatrix);
			groundTemplate.set_size(para.szGround + para.szROIMatrix - 1,para.szGround + para.szROIMatrix - 1);
			zeroGround.set_size(para.szGround,para.szGround);
			iFoIThres.set_size(para.szGround,para.szGround);
			personRoICube.set_size(para.szGround,para.szGround,MaxNumPeople);

			RoomS.set_size(para.szGround,para.szGround);
			RoomST.set_size(para.szGround,para.szGround);	
			personST.set_size(para.szGround,para.szGround,MaxNumPeople);
			RoomBV.set_size(para.szGround,para.szGround);

			iFoINum.set_size(para.szGround,para.szGround);
			iFoIMaskMulti.set_size(para.szGround,para.szGround);
			iFoIMaskDiv.set_size(para.szGround,para.szGround); 

			interactantCircleMask.set_size(para.interactantRadius*2+1,para.interactantRadius*2+1);

			bViewMask.set_size(para.bViewMaskMaxR*2+1,para.bViewMaskMaxR*2+1);
			bValue.set_size(para.bViewMaskN); // Output Array
			sectorMaskTemplate.set_size(2*para.bViewMaskMaxR+1,2*para.bViewMaskMaxR+1,para.bViewMaskN);

#pragma region Initialization

			groundTemplate.zeros();
			zeroGround.zeros(); 
			iFoIThres.fill(1);
			RoomST.zeros();	
			personST.zeros();

			// Load data
			// gantian Load Data <2>
			/*loadPeopleInfo(dataLoad,szSeq,szPeople);
			dataLocX.set_size(szPeople);
			dataLocY.set_size(szPeople);
			dataLocOri.set_size(szPeople);*/

			// Result
			/*resCenterNo.set_size(szSeq);
			resCenterNo.zeros();
			resInteractant.set_size(szPeople/2,szPeople,szSeq);
			resInteractant.zeros();
			resCenterLoc.set_size(szPeople/2,2,szSeq);
			resCenterLoc.zeros();*/

			interactionCenterX.set_size(MaxNumPeople/2);
			interactionCenterY.set_size(MaxNumPeople/2);
			interactionValue.set_size(MaxNumPeople/2,MaxNumPeople);

#pragma endregion

#pragma region Create Template    
			if(!para.hasROITemplate)
				//if(!(personROICubeTemplate.load("personROICube.bi", arma_binary))) // Do not have the ROI template binary file
			{
				//////////////////////////////////////////////////////////////////////////
				// Create Orientation Template

				//personROICubeTemplate.resize(sizeROIMatrix,sizeROIMatrix,sliceNum);
				//personROICubeTemplate.zeros(); 
				arma::Mat<eT> gaussMatrix(para.szROIMatrix*2+1,para.szROIMatrix*2+1);
				gaussMatrix = getGaussianMatrix(para/*szROIMatrix+1,szROIMatrix+1,sigmaXSquare,sigmaYSquare,szROIMatrix*2+1*/);

				arma::Mat<eT> gaussMaskMatrix(para.szROIMatrix,para.szROIMatrix);

				eT everyOri = 360.0f/para.sliceNum;
				arma::Mat<eT> saveROIMatrix(para.szROIMatrix,para.szROIMatrix);
				arma::Mat<eT> personROIMatrix(para.szROIMatrix,para.szROIMatrix);
				personROIMatrix.zeros();
				for(int i=0;i<para.sliceNum;i++)
				{

					//personROIMask(szRadiusROI+1,szRadiusROI+1,everyOri*i,deltaOri,(float)szRadiusROI,personROIMatrix);
					personROIMask(
						personROIMatrix,
						everyOri*i,
						(float)para.szRadiusROI,
						para
						);

					//personROICubeTemplate.slice(i) = personROIMatrix % gaussMaskMatrix;       

					gaussMaskMatrix.zeros();
					int rU,rD,cL,cR;
					rU = para.szRadiusROI-(int)(para.interactionCenterDis*sin((everyOri*i*PI)/180));
					cL = para.szRadiusROI-(int)(para.interactionCenterDis*cos((everyOri*i*PI)/180));
					rD = rU + para.szROIMatrix-1;
					cR = cL + para.szROIMatrix-1;

					gaussMaskMatrix = gaussMatrix.submat(rU,cL,rD,cR);
					saveROIMatrix = personROIMatrix % gaussMaskMatrix;  

					convertS.str("");
					convertS << "..\\ROITemplate\\" << i ;
					saveROIMatrix.save(convertS.str(),arma_binary);                        
					//saveROIMatrix.save(convertS.str(),raw_ascii);                        
				}
				//personROICubeTemplate.save("personROICube.bi", raw_binary);

				//return 0;
			}

			if(!para.hasInteractantMaskT)
			{
				//////////////////////////////////////////////////////////////////////////
				// Create Interactant Detection Circle Template

				//timer_arma.tic();
				interactantCircleMask.ones();
				int boundR = 2*para.interactantRadius+1;
				int boundRSquare = para.interactantRadius*para.interactantRadius;
				for(int i=0;i<boundR;i++)
				{
					for(int j=0;j<boundR;j++)
					{
						if( ((i-para.interactantRadius)*(i-para.interactantRadius) + (j-para.interactantRadius)*(j-para.interactantRadius)) > boundRSquare)
							interactantCircleMask.at(i,j) = 0;				
					}
				}
				//cout<< timer_arma.toc() << endl;
				convertS.str("");
				convertS << "..\\ROITemplate\\interactantCircleMask";
				interactantCircleMask.save(convertS.str(),arma_binary);

				//interactantCircleMask.save(convertS.str(),raw_ascii);
			}
			else
			{
				convertS.str("");
				convertS << "..\\ROITemplate\\interactantCircleMask";
				interactantCircleMask.load(convertS.str(),arma_binary);
			}

			if(!para.hasBestViewMaskT)
			{		
				arma::Mat<eT> atan2Mat(para.bViewMaskMaxR*2+1,para.bViewMaskMaxR*2+1);

				for(int i=0;i< para.bViewMaskMaxR*2+1;i++)
				{
					for(int j=0;j< para.bViewMaskMaxR*2+1;j++)
					{
						atan2Mat.at(i,j) = atan2f(eT(j-para.bViewMaskMaxR),eT(i-para.bViewMaskMaxR));
					}
				}

				eT lowAngle,upAngle;
				eT deltaAngle = 2*PI/para.bViewMaskN;
				for(int iBVN = 0;iBVN<para.bViewMaskN;iBVN++)
				{
					lowAngle = -PI + iBVN*deltaAngle;
					upAngle = lowAngle + deltaAngle;

					bViewMask.zeros();
					for(int i=0;i< para.bViewMaskMaxR*2+1;i++)
					{
						for(int j=0;j< para.bViewMaskMaxR*2+1;j++)
						{
							bViewMask.at(i,j) =  (atan2Mat.at(i,j) <= upAngle) && (atan2Mat.at(i,j) > lowAngle);
						}
					}

					//cout<< timer_arma.toc() << endl;
					convertS.str("");
					convertS << "..\\ROITemplate\\bViewMask_" << iBVN;
					bViewMask.save(convertS.str(),arma_binary);			
					//bViewMask.save(convertS.str(),raw_ascii);			

				}
			}
			else
			{
				for(int iBVN=0;iBVN<para.bViewMaskN;iBVN++)
				{
					convertS.str("");
					convertS << "..\\ROITemplate\\bViewMask_" << iBVN;
					sectorMaskTemplate.slice(iBVN).load(convertS.str(),arma_binary);		
				}	
			}

#pragma endregion
		}
		~HM()
		{
		}
	};

}